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ABSTRACT 


The idea of using a passive end point motion constraint 
to calibrate robot manipulators is of particular interest 
because no measurement equipment is required. The accuracy 
attained using this method is compared to the accuracy 
attained by an unconstrained calibration using computer 
Simulated measurements. A kinematic model is established for 
each configuration using the Denavit-Hartenberg methodology. 
The kinematic equations are formulated and are used in the 
computer Simulated calibration to determine the actual 
kinematic parameters of the manipulator. The results are 
discussed in terms of the effect of measurement noise and the 
number of experimental observations on the accuracy of 
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I. INTRODUCTION 


The goal of using robot manipulators as the key link in 
flexible automated manufacturing systems has’ presented 
engineers with a variety of significant problems. For a six 
degree of freedom robot, the position and orientation of the 
manipulator end point must be specified for each pose. 
Accuracy and repeatability are the yardsticks of a robot’s 
performance. Accuracy is the measure of the robot’s ability to 
move to a commanded position in its workspace. Repeatability 
is the measure of the robot manipulator’s ability to return to 
a previously learned position. Presently, robots that are used 
in industrial applications display adequate repeatability, but 
Ge@memot exhibit satisfactory levels of accuracy. For most 
industrial robots, repeatabilities of the order of 1 mm or 
better can be attained while the positioning accuracy may be 
pee oy as much as 1 cm [Reft. lip. 14]. For on-line programming 
applications such as the traditional automated pick and place 
operations where the robot manipulator must be taught the 
desired motion, adequate repeatability alone is sufficient. 
However, as the concept of off-line programming was developed 
as a means of automatically generating robot control programs 
for tedious applications that previously would have involved 


large numbers of taught tasks, the low levels of accuracy that 


robot manipulators could attain became a major roadblock to 
their widespread usage. 

There are several factors that adversely influence the 
accuracy of robot manipulators. Among them are: temperature 
variations, gear backlash and harmonics, compliance in links 
and joints, steady state errors in the joint servo 
controllers, and inaccurate knowledge of the manipulator’s 
kinematic parameters. Experience has shown that the most 
prevalent source of error is inaccurate knowledge of the 
kinematic parameters that the robot controller has of the 
manipulator arm. This work deals primarily with ene 
identification of the variations in the kinematic parameters 
of the model that the rvebot Cconerollerwiace 

Even small variations in these kinematic parameters can 
cause significant error in the manipulator end )pemer 
placement. The calibration process identifies the actual 
kinematic parameters of the model and uses them to update the 
robot controller’s model so that the manipulator end point may 
be placed into a commanded position with greater accuracy. In 
calibration tests performed by Mooring, Roth, and Driels [Ref. 
2:p. 6] and several others, it has been shown that correction 
of the kinematic errors resulted in improvement in accuracy to 
the same order of magnitude as the repeatability. 

The process of robot manipulator calibration is 
characterized by foltir major steps: modelling, measurchicm= 


identification, and correction. The first step an wee 


ealibrationeprocess 16 to form a valid kimematic model of the 
manipulator. The model is the fundamental relationship between 
the manipulator’s kinematic parameters and the resulting end 
effector pose. The manipulator model may take two basic forms. 
The forward kinematic model is used to compute the end 
effector pose given the joint variable data. The inverse 
kinematic model is used to determine the joint displacements 
for a given pose. The kinematic model is constructed using the 
Denavit-Hartenberg method with modifications. The resulting 
model is used to define an error quantity based on the nominal 
kinematic parameter set and the unknown actual kinematic 
parameter that need to be identified. 

Measurement involved physically moving the manipulator end 
effector to various locations in its workspace and recording 
the corresponding joint displacements. There are a number of 
methods that have been used to obtain the data necessary for 
manipulator eal Poratsone Theodolites [Ref. Cale laser 
inferometers [Ref. 4], coordinate meaSuring machines [Ref. 5], 
and many other techniques can be used depending on the 
constraints imposed by the desired level of accuracy, size, 
ease of use, and cost. Alternatively, joint variable data and 
pose information can be obtained through computer simulation 
with the use of a random number generator routine. This was 
the approach employed in this research. 

In the identification phase, the task is to identify the 


set of model parameters that allow the poses computed from the 


model to most closely match the meaSured data. This is 
accomplished through the use of a gradient based Levenberg- 
Marquardt algorithm that used the collected pose information 
to identify the actual parameters by systematically changing 
the nominal parameters to reduce the previously defined error 
quantity. There are several factors that influence the 
identification process. These factors are the type of 
identification routine used, the initial values of the 
parameters to be determined, the number of poses taken, the 
influence of measurement accuracy and noise, encoder noise, 
the choice of measurement configuration, and the attainable 
range of joint displacements used during the observations. 
These effects are discussed in detail at a later time. 

Pina aie in the correction step, these identified 
kinematic parameters are used to update the robot controller’s 
model. This process, however, is not without its own unique 
set of problems. Normally, an inverse kinematic solution using 
the actual kinematic parameters is employed to convert the 
desired off-line locations in the task space to modified 
locations in the manipulator’s own joint space. The robot has 
an inverse kinematic solution for the nominal model, but may 
have to develop its own solution using the actual@model jth 
issues are beyond the scope of this research and were not 
addressed. 

The purpose of this research was to compare the accuracy 


attained for two different computer simulated calibration 


methods. The first method involves using an unconstrained 
manipulator end point and the second method employs a passive 
end point motion constraint called a ballbar. These computer 
Simulated calibrations were performed on the Model G Compact 
Master-Slave Manipulator shown in Figure 1. The Model G 
Master-Slave Manipulator is a six degree of freedom 
manipulator arm with five revolute joints and one prismatic 
joint (5R1P). This manipulator is designed to reproduce the 
natural movements and force of the human hand. The manipulator 
end point will move in exactly the same manner as the operator 
moves the manipulator handle. The motion is constrained only 
by the dimensional limits of the manipulator itself. The 
forces produced at the end point will be the same as those 
forces applied at the handle with the exception of minor 
losses due to unbalance and friction. This manipulator was 
chosen for these calibrations because of its usefulness for 
experiments that are concerned with probing of objects that 
can not be viewed during the probing operation to acquire 
Somemee information. 

The format of the remainder of this thesis will be to 
first conduct an in-depth examination of theory applicable to 
robot calibration. This will be followed by an analysis of the 
two calibration methods used and the unique problems with each 
method. Next, will be a discussion of the results obtained 
and, finally, the conclusions drawn from this research will be 


stated. 
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II. THEORY 


A. THE DENAVIT-HARTENBERG METHOD WITH MODIFICATIONS 

As was discussed in the Introduction, the starting point 
for any calibration process is the establishment of a 
representative model of the manipulator. There are currently 
a number of methods of generating the forward kinematic model 
of a serial link manipulator. The technique that was used to 
define the spatial orientation between objects and various 
locations in the manipulator working volume is the Denavit- 
Hartenberg method [Ref. 6] with modifications proposed by 
Hayati [Ref. 7], Mooring [Ref. 8], and Wu [Ref. 9] to handle 
Situations in which consecutive joint axes are nominally 
parallel. The basic concept is to place a coordinate frame on 
Saem Of the manipulator links using a set of rules that 
defines the origin of the frame and its orientation. The 
position of consecutive links is specified by a homogeneous 
transformation matrix, which transforms the frame fixed on 
link n-1 into the frame fixed on link n. This transformation 
is composed of more fundamental transformations representing 
three basic translations along the x, y, and z axes and three 
rotations about those same axes. These 4 x 4 matrix 


transformations are expressed as follows: 


I UN Gres 
Oe 
: = (1) 
Trans. (347 22.) 0012 
Om0s0..0 
1 0 0 0 
Oeese.. -Sind. 0 
ROT (x, Oy) = : i (2) 
0 sin®, cos0, 0 
0 0 0 i 
cos0,, 0 sinOy, 0 
0 i 0 0 
ROT (y, Oy) = (3) 
(Say: -sin®y 0 cos®,, 0 
0 0 0 if 
cos0 sane eau 
sin@ os8, 0 0 
ROT (2, Oe) = be ee (4) 
0 0 ilo) 
0 0 C- 


where trans (x, y, z) describes a translation given by the 
vector r = [x, y, z]’ and ROT (x, 90,) describes a 2ctaeron os 
8, about the x-axis of the coordinate frame. 

With the aid of Figure 2, the  Denavat-Hareens— we 
transformation methodology can be illustrated. First; seg sae 
of joint motion must be identified and the z-axis must be 
aligned with the axis of joint motion. Next, the common normal 
between consecutive joint axes must be identified. 

Then, the origin of coordinate frame n is located at the 


intersection of joint axis n+l and the common normal betweeu 


joint | Joancant 1 


Link ntl 


link n-1 C link a 








Figure 2. Placement of Coordinate Frames. 


axes n+l andn. the z axis of coordinate frame n is always 
aligned with joint axis n+l and the x axis is always aligned 
along the common normal between consecutive joint axes. 
Transforming frame n-1 to frame n is accomplished by the 
following sequential steps: 
¢ Rotate frame n-1 about axis z,, by an angle 9,, the joint 
angle. 
* Translate along axis z,_, a distance d,, the offset. 


* Translate along the rotated x,, axis, a distance aye 
link length. 


* Rotate about axis x, by an angle @, the twist angle. 


* Rotate about axis y, by an angle B. 


Incorporating these rules with the transformation matrix 
format specified in Equations 1, 2 and 3, the transformamaen 


from frame n-1l to frame n is expressed in the following form: 
An = ROT(z,®,) Trans (z,d,) Trans (x,a,) ROT(x,a,) ROT(y,8,) (5) 


Performing the matrix multiplications gives the resulting 


EOE: 
CO. Cp -s0-sea- sp -S8.Ca, cO8_SP +56 Sa ch. a ae 
Pers S8,CB,+C8,Sa,S5B, C8,Ca,  S0,SB,-Cc®,Sa,CB, a,58,| (6) 
-@a Spr Sa, Ca cp d, 
0 0 0 i 


In most cases, four out of the five transformations are 
necessary to transform frame n-1 into frame n. For revolute 
joints, the parameters dn, a, and @, are constants dictated By 


the geometry of the manipulator and 8, is the angular joint 
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variable. The parameter $B is defined only in cases where 
consecutive coordinate axes are parallel and, in these 
instances, d,, is normally set to zero. When consecutive axes 
are parallel, there is no unique common normal. The 6, 
parameter allows for small amount of inclination between the 
Mees efor Prismatic joants, the location of the origin of the 
coordinate system is determined by extending the axis so that 
it intersects the axis of the next joint. This makes the 
length of the common normal, a,, and the next joint offset, 
dis31, both equal to zero. Therefore, for prismatic joints, d, 
is the joint variable and the link geometry is described by 6, 
amie O, . 

imeeorder for a robet manipulator to have complete 
dexterity in its working volume, it must have six degrees of 
freedom. For a six joint six link manipulator, the 


MeanstoOrmation from frame 5 to frame 6 takes the form 


Peo 27 Oe) ROM a.) ROT (ou jerrans (pi, p., P<) (7) 


where the rotations are sequentially defined as roll, pitch 
and yaw [Ref. 10]. The transformation from the base coordinate 


frame to the manipulator end link is: 


T6= A1A2A AASAG (8) 
Any suitable calibration model must be, in Everett’s terms 
[Ref 11], complete. Completeness refers to the model’s ability 
to relate joint displacements to the tool pose for a 


manipulator while allowing for the arbitrary placement of the 
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world coordinate frame and arbitrary assignment of the 
manipulator’s zero position. In other words, the model must 
have the proper number of identifiable parameters to account 
for variations in those parameters. The required number of 
independent kinematic parameters is the same as the number of 
constraint equations necessary to specify the tool pose and 
joint frames. Mooring, Roth and Driels [Ref. 2:p. 43] have 
concluded that for each revolute joint, four independent 
kinematic parameters are needed and for each prismatic joint, 
two independent kinematic parameters are required. The 
required number of independent kinematic parameters N, can be 


determined from the following equation. 


N= 4R+2P+6 (9) 


R is the number of revolute joints and P is the number of 
prismatic joints. An additional six parameters are specified 


in order to obtain an independent tool frame location. 


B. PARAMETER IDENTIFICATIONS 

The flowchart in Figure 3 outlines the calibration prog = 
up through the identification step. First, the range of motion 
for each joint and the number of observations to be made must 
be determined. Next, sets of joint variables for each 
observations are obtained with the use of a random number 
generator program. The forward kinematic model of the 5RI1P 


manipulator is then applied to generate pose data for each of 


eZ 


Joint Motion Range 
Number of Observations 


Generate Joint Variable Data 


Nominal Parameters —__ Compute Actual 
+ AP End Effector Position 


Encoder Noise ——=> CB CB <—— Measure Noise 






Nominal Parameters —> Non-Linear Least Squares 
Parameter Identification 


Identified Kinematic Parameters 


Figure 3. Flowchart of Calibration Process. 
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the observations using the joint variable data and manipulator 
link parameters. The kinematic parameters used in this forward 
kinematic solution are the nominal kinematic parameters plus 
a known error parameter, AP. The error parameter can take the 
form of a length error or an angular error as iS appropriate 
for each of the parameters that is to be identified. The 
result of this application is a Simulated known manipulator 
pose for each of the joint variable sets. The random noise of 
measurement and encoder noise were superimposed on the pose 
data and joint variables respectively. 

The simulated observation data and the nominal kinematic 
parameters are the inputs to an identification program ID6. 
ID6 initializes the nominal kinematic parameters and feeds 
them to an identification subroutine ZXSSQ which numerically 
estimates the gradient and uses it to produce improved 
predictions of the kinematic model parameters. ZXSSQ employs 
a subroutine that takes the current parameter estimates and 
calculates an error between the model predictions and the 
Simulated observation data. ZXSSQ uses the error to determine 
the gradient. The cycle continues until convergence criteria 
is met. The parameter estimation is treated as _ an 
unconstrained non-linear optimization problem. Figure 4 shows 
a Llowchart of the preececee 

ZXSSQ is a finite difference, Levenberg-Marquardt routine 
that 1s tailored for non-linear least squares problems [Ref. 


2:pp. 135-139]. The best way to illustrate its use is through 
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Nominal Parameters 





Nominal Parameters 


+ + 
Known AP Unknown AP 






Joint 
Variable Data 






Kinematic Model Kinematic Model 






Measured Pose Matrix 


Estimated Pose Matrix 
Tm T 





AT=T-TM 
Pose Error 


ZXSSQ 
Changes Unknown AP 
To Minimize Pose Error 





Figure 4. Flowchart of Identification Process. 
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an example. Consider the simple two link manipulator in Figure 
5. What values of 0, and @, will put the manipulator end point 
at point Q? If P=F(@,, 9,), @, and @, are known link lengths, 
and Q is a known position in the two dimensional workspace, 
the problem is to find 0, and 8, such that the quantity z=(P-Q) 
approaches zero. In two-dimensional matrix notion, the 


equations are as follows: 


x Ee eo. | cos 07 th cecnto 0 aac (10) 


“ “ly, | | 2,-0,} | @,sin@,+@,sin (0,+0,) -9, 


Where x, and y, are the difference between the estimated and 
known parameters in the x and y directions respectively. The 
Levenberg-Marquardt algorithm uses this error quantity to 
numerically estimate the gradient and produces updated @, and 
8, values. The process continues until predetermined 
convergence criteria are met and @, and 9, will be the values 
needed to put the manipulator end point at point Q. A more 
detailed explanation of the ZXSSQ algorithm is given in 
Appendix A. 

According to Equation 9 for the 5RIP manipulator Useqia 
this study, it would be expected that 28 kinematic parameters 
would have to be identified. It will later be seen that this 
number will have to be altered to meet the particular needs of 
the measurement method employed and to achieve satisfactory 


parameter identification. 


nC 





Figure 5. Simple 2-Link Manipulator. 
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III. CALIBRATION METHODS 


A. UNCONSTRAINED METHOD 

This method involves establishing a valid forward 
kinematic model using the Denavit-Hartenberg methodology and 
using this model to generate the manipulator poses that will 
be input to the identification program. Employing the Denavit- 
Hartenberg criteria previously discussed to the Model G 
Master-Slave Manipulator produces the model shown in Figure 6. 
The location of the world coordinate is arbitrary and) @eee 
fixed in the position shown only because it was a convenient 
reference frame for measurements. The location of the base 
frame of the manipulator is also arbitrary as long aS Agee 
aligned with the first joint axis. The remainder of the 
coordinate frames were allocated in accordance with the 
Denavit-Hartenberg Method. The transformation from frame 2 to 
frame 3 required the use of the parameter £8; because 
coordinate frames 2 and 3 are nominally parallel [Ref. 12]. 
The definition of frame 6 is arbitrary and, in general, the 
transformation from frame 5 to frame 6 requires three 
translations and three rotations. However, since frame 6 was 
chosen to be offset from the origin of coordinate frame 5, dee 
orientation is undefined. Therefore, only three parameters are 


required to transform from frame 5 to frame 6 and at least one 


ES 





Figure 6. Denavit-Hartenberg Model of Master-Slave 
Manipulator (Unconstrained Method) 
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of these parameters must be a displacement otherwise no 
movement of the origin of frame 5 would occur. 

The parameters $,, px, and pz, were chosen to define the 
transformation from frame 5 to frame 6 because the identified 
deviation from the nominal value of o, 1S composed of the 
encoder offset 60, and the constant offset 66, as well as the 


joint variable @,. The definition of 6, is as follows: 
6, = 0, + 08, + 36, (11) 


The transformation from frame 5 to frame 6 can be expressed in 


a reduced form 
A= RO (2, 0.) TRANGE@Ee, Oreo (12) 


As was discussed previously, in order for a calibration 
model to be valid, it must satisfy the completeness criteria 
specified in Equation 9. The required number of identifiable, 
independent kinematic parameters must equal the number of 
constraint equations needed to define the tool pose [Ref. 2:p. 
42). Using the completeness criteria, it is expected that 
there must be 28 identifiable kinematic parameters in order to 
have a valid model of a 5SRIP manipulator. However, because 
only three parameters instead of six were used to specify the 
transformation from frame 5 to frame 6, the required number of 
independent kinematic parameters is 25 for this calibration 
process. 

There were some unique problems encountered applying the 


Denavit-Hartenberg methodology to the Model G Master-Slave 
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Manipulator. The prismatic joint, in particular, requires some 
modifications to the Denavit-Hartenberg model. Because the 
location of frame 3 is defined by the common normal between 
axes 4 and 5 and the prismatic joint axis (frame 3) is not 
fixed in space, the prismatic joint axis is free to move and 
memo ves through the origin of frame 3 (Ref. l:p. 18]. 
Consequently, the parameters a, and d, are always zero. In 
addition, the parameter 9, must be set at a constant value 
because it cannot be identified independently from 9, for this 
manipulator configuration. Table I shows the table of the 
nominal kinematic parameters. The parameters that are defined 


to be zero are in boldface type. 


TABLE I. NOMINAL KINEMATIC PARAMETER TABLE 
(UNCONSTRAINED CASE) 





Having established a valid, working model of the Model G 
Master-Slave Manipulator, the task was then to obtain joint 


variable data for a variety of manipulator poses. This was 


Zak 


accomplished using Program JOINT. Program JOINT uses a random 
number generator subroutine to generate the joint variable 
data and then stores it ina data file called TELE-VAR.DAT. 
Program JOINT is run a second time to obtain joint Vanuaaiome 
data that will be used in a verification program that will be 
described in more detail later. This second set of joint 
variable data is stored in file POSEVER.DAT. 

The next step is to generate pose information for the 
Model G Manipulator simulation. Program POSE reads the joint 
variable data from file TELE-VAR.DAT and the table of nominal 
kinematic parameters from file INPUT.DAT and computes the 
manipulator pose uSing a forward kinematic solution. The set 
of joint variables and the corresponding manipulator end point 
pose information are stored in file TELE-POS.DAT. In program 
POSE, estimates of measurement noise and the encoder offsets 
are added to thewdata throughe INPUT Pal 

The actual kinematic parameters are identified by program 
ID6, using the previously discussed non-linear least squares 
method. Program ID6 consists of three Main components. 3iae 
first component is where the nominal kinematic parameters are 
read from INPUT.DAT and the pose data for each observation are 
read from TELE-POSE.DAT. The program then defines the initial 
values of the model and the parameters required by the 
LOehibIet Teac ion subroutine ZXSSO are initialized. The 
identification subroutine ZXSSO is the second major component 


of program ID6. The details of how ZXSSQ works can be found in 


ae 


the parameter identification section of Chapter II and in 
Appendix A. ZXSSQ iteratively estimates the gradient and uses 
the estimate to produce an updated approximation of the model 
parameters. The cycle continues until the kinematic parameters 
are identified consistently to four significant figures. The 
third major component of program ID6 is the subroutine TELE- 
ARM which takes the current estimate of the model parameters, 
computes a forward kinematic solution using the estimated 
parameters, and then calculates the error between the model 
prediction and the measured pose data. This error is, in turn, 
used by ZXSSQ to determine the gradient. The output of program 
Mow 1S file RESULT.DAT which consists of the actual, 
identified kinematic parameters of the manipulator and the 
calculated RMS difference between the nominal and identified 
kinematic parameters. The RMS quantity is broken down into 
length and angular error parameters (K, and K,) respectively. 
These error parameters reflect the accuracy of the 
identification process. 

The final stage of the computer simulated calibration 
process is a verification program designed to determine the 
Hecwnacy that the manipulator could attain if the identified 
kinematic parameters were to replace the nominal parameters in 
the manipulator’s controller. Program VERIFY reads the nominal 
kinematic parameters from INPUT.DAT and the identified 
parameters from RESULT.DAT and computes separately for each 


set of parameters a forward kinematic solution. These 
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solutions are used to calculate the differential position and 


Orbentat Lonm@mat rix- 


Qin OO Chis 

Om ORS orra 

Nes ais 
-0, 0, WONMaz 


0 SS ea, 


(13) 


The position error is calculated as follows: 


POSERR = dx’ +dy?+dz’ (14) 


The position error is indicative of the accuracy of the 
calibrated manipulator. Figure 7 shows a flowchart of the 
programs used in the simulated calibration process. These 


programs and data files can be reviewed in Appendix B. 


B. CONSTRAINED (BALLBAR) METHOD 

The ballbar method involves the use of a passive end point 
motion constraint to obtain pose data. The end point of the 
manipulator is connected to a fixed point on a table by means 
of a ballbar of known length. Figure 8 shows the Model G 
Master-Slave Manipulator configuration with the ballbanwiam 
length 552.8 mm attached. This ballbar length was obtained by 
fixing the ballbar at a location very near the manipulator end 
point when it is in the zero position. The other end of the 
bar waS connected to the manipulator end point. The ballbar 
was then cycled through its reachable volume while constrained 


by the dimensional limits of the manipulator. This approximate 
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POSE POSEVER.DAT 


TELE-VAR.DAT 


INPUT.DAT =" ID6 







RESULT.DAT 


Figure 7. Programs Used in Unconstrained Calibration Process 
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bar length was chosen because it maximized the possible range 
of joint displacements. 

The coordinate frames were allocated in accordance with 
the Denavit-Hartenberg criteria. With the exception of the 
world coordinate frame, the coordinate frames were positioned 
exactly as they were for the unconstrained calibration. 
However, special consideration must be given to the choice of 
world coordinate frame. If it is assumed that all the Model G 
Manipulator joint displacements are fixed, a rotation of frame 
O relative to the world frame results in a change of the 
manipulator end point (the origin of frame 6) coordinates in 


the x,, yy, zZ, frame. The distance between the world coordinate 


frame and manipulator end point is the fixed length of the 
ballbar and it remains unchanged. Since the relative rotation 
between the world frame and the base frame cannot be measured 
the conclusion is that this rotation cannot be identiimege 
Hence, it is logical that the world coordinate frame selected 
be orthogonal to the base frame. The location of the base 
frame is arbitrary as long as the z, axis is aligned with the 
frst JOUne qaxi1s. 

The transformation from the world coordinate frame to the 
base frame is a function of the parameters x,, y,, and z, only. 


Because parameters 2 and d, are measured in the same 


direction, they cannot both be identified. Therefore, the 
parameter z, is set to zero. The transformation from the world 


coordinate frame to the base frame can be expressed as follows 
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Figure 8. Denavit-—Hartenberg Model of the Master-Slave 
Manipulator (Ballbar Method) 
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A, = TRANS (X,, Y,, 0) (15) 


In addition, it has been determined the parameters x,, y,, and 
66, are not independent. The parameter 660, cannot be identified 
and was set to zero. The table of nominal kinematic parameters 
for ball method calibration of the Model G Manipulator is 
shown in Table II. The parameters in bold face type were not 
identified in the calibration process. For the ballbar 
calibration method, there are 22 kinematic parameters that 
must be identified. 


TABLE II. NOMINAL KINEMATIC PARAMETER TABLE 
(CONSTRAINED CASE) 


xy Vw 2. 
mm mm mm 


1542.7 | -132.95| 0.0 | 






As in the unconstrained calibration process, the simulated 


ballbar method calibration of the Model G Manipulator is 
accomplished with a series of computer programs. A flowchart 


of the programs used is shown in Figure 9. The ballbar length 
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Figure 9. Programs Used in Constrained (Ballbar) 
Calibration Process 


RESULT.DAT 
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is fixed at 552.8 mm and at each end of the bar iS augeenee 
joint capable of 930° of solid angle rotation..Onewend ofvege 
bar is attached to a fixed point in space and the other end is 
attached to the end flange of the Model G Manipulator. Pose 
information is created in program TELEBAR by using a4 ran@om 
number generator routine to generate the angles that the bar 
makes with the z, and y, axes (Figure 9). The position of the 


Datla senna Podmrs ts 
Cees cae Oo, ae (16) 
where the transformation R is defined as follows 
R= ROT (Zz, 9 \RROT Cy Oona aan) (17) 


For each pose, the distance d from the manipulator end 
point to the origin of the world coordinate frame is 


calculated using 


Gane = yy - ee (18) 


where x, y, and z are the coordinates of the manipulator end 
point in space relative to the world coordinate frame. Program 
TELEBAR employs the previously discussed non-linear least 


squares algorithm ZXSSQ to minimize the function 
F,=|d,-¢| (19) 


wheres @ is thew@®eng@phewoef the ballbar subroutine Zs 
iteratively estimates a gradient and produces an approximation 


of the joint displacements necessary to establish the current 
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Figure 10. Kinematics of Ballbar 


pose. ZXSSQ uses subroutine TELE-ARM to compute the forward 
kinematic solution of the manipulator using the current values 
of joint displacements. TELE-ARM calculates F, which is used 
by ZXSSQ to determine the gradient. The cycle continues until 
the joint variables for the pose are consistently identified 
to four significant figures. The joint displacements are 
Seeored in file TELE-SOLN.DAT. Program TELEBAR iS run a second 
time to generate joint variable displacements for use in the 
verification phase. The second set of joint variables is 
stored in file POSEVER.DAT. 

Program FORWARD is used to check the validity of the joint 


variable data generated by program TELEBAR. Program FORWARD 


SL 


computes the forward kinematic solution for the Model G 
Manipulator for each set of joint displacements. The distance 
d from the manipulator end point to the world coordinate frame 


is 
d=yxe+y?+2? (19) 


where x, y, and z are the coordinates of the manipulator end 
point relative to the world coordinate frame. If d equals the 
length of the ballbar, the corresponding set of Joint 
displacements is valid. Program FORWARD is not a part of the 
calibration process, but it iS an expeditious way to check the 
joint variable data. 

As in the unconstrained calibration process, the 
identification of the actual kinematic parameters of the 
manipulator is accomplished by program ID6. Program ID6 reads 
the nominal kinematic parameters from INPUT.DAT and the pose 
PntoOrmMaweLon from  TELE-SOLN. DAT. The actual kinematic 
parameters of the manipulator are stored in file RESULT.DAT. 
Program VERIFY calculates the accuracy of the manipulator 
using the actual identified kinematic parameters as was 
explained earlier. These computer programs are shown in 


Appendix B. 


SZ 


IV. DISCUSSION 


In order to obtain a satisfactory comparison of the two 
@elibration methods, a number oof computer simulated 
calibrations were performed on each configuration. In these 
Simulations, the independent variables were the number of 
observations taken and the level of measurement noise present. 
The dependent variables were the accuracy of parameter 
m@enmtification and the manipulator accuracy uSing the 
identified kinematic parameters. The resultS are shown in 
meogure i} through Figure 18. For the unconstrained 
configuration, the low noise value was set at 0.1 mm. The 
accuracy of parameter identification and the position error 
were each separately plotted against the number of 
observations. The same procedure was repeated with the 
measurement noise increased ten times to 1.0 mm. The entire 
process wasS again repeated using the low and high noise levels 
memethe ballbar configuration. 

In general, accuracy of parameter identification increased 
and the position error decreased as the number of observations 
increased regardless of noise level and calibration 
configuration used. For the low noise level, the calibrated 
manipulator accuracy is of the same order of magnitude as the 
attainable repeatability COm iS Y mia) fOormemnis. type jot 


manipulator. This suggests that in the presence of a readily 
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attained low level of measurement noise (0.1 mm) the 
manipulator accuracy attained using the ballbar method is 
roughly equal to that attained using the unconstrained method 
and is, in fact, quite Satisfactory. In addteven wees 
manipulator accuracy can be improved by increasing the number 
of observations taken during the measurement phase of the 
Calibration. There reaches a point, however, when making 
additional observations produces no marked improvement in 
manipulator accuracy. Table III is a table of the nominal and 
identified kinematic parameters for the unconstrained 
calibration using low noise and 60 observations. Table IV 
shows the same parameters for the ballbar calibration using 
low noise and 55 observations. 

When the high noise level (1.0 mm) was used, the position 
error obtained using the ballbar method was unsatisfactory 
even when a large number of observations were made. For the 
unconstrained case, the position error wasS an order of 
magnitude higher than it was using the low noise level. The 
ability to obtain high manipulator accuracy 1s dieser 
dependent on the effectiveness with which noise can be 
eliminated fhisen the measurement process. ial ac imras 
calibrations, the reduction of measurement noise iS a pivotal 
step in the process. 

For calibrations of serial link manipulators, the balla 
method has been proven to be a convenient alternative to the 


use of expensive ancillary measurement equipment. With this in 
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Figure 11. Accuracy of Parameter Identification/Low Noise 
(Unconstrained Method). 
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Figure 12. Manipulator Accuracy/Low Noise 
(Unconstrained Method). 
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Figure 13. Accuracy of Parameter Identification/Low Noise 
(Ballbar Method). 
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Figure 14. Manipulator Accuracy/Low Noise (Ballbar Method). 
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Figure 15. Accuracy of Parameter Identification/Noise x 10 
(Unconstrained Method). 
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Figure 16. Manipulator Accuracy/Noise x 10 
(Unconstrained Method). 
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Figure 17. Accuracy of Parameter Identification/Noise x 10 
(Ballbar Method). 
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Figure 18. Manipulator Accuracy/Noise x 10 
(Ballbar Method). 
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TABLE III. NOMINAL AND IDENTIFIED KINEMATIC PARAMETERS USING 
UNCONSTRAINED METHOD WITH LOW NOISE AND 60 OBSERVATIONS 


hand, 


frame 


Seal Or. 


world 


HO Nt 


PARAMETER NOMINAL VALUE IDENTIFIED VALUE 


EOUU DS 
P2624 
54560 
sVO01S 


0 
0 
) 
a0) 
a0 
aU 
0 
0 





an effort was made to determine which world coordinate 
locations yielded the smallest manipulator position 
Referring back to Figure 8, the x-coordinate of the 
coordinate frame which is the distance to the first 


axis was set at each of the following three values: 


eg 


TABLE IV. NOMINAL AND IDENTIFIED KINEMATIC PARAMETERS USING 
BALLBAR METHOD WITH LOW NOISE AND 55 OBSERVATIONS 


PARAMETER NOMINAL VALUE IDENTIFIED VALUE 


1543.20468 


=32, 137 ee 





x=1292.7 mm, 1542.7 mm, and 1792.7 mm. At each of thesewe 
coordinate levels, the world coordinate frame was positioned 


at each node of a five by five y-z grid and the position error 
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was computed at each location. Figure 19, Figure 20, and 
Figure 21 illustrate the results. The location of the world 
coordinate frame does not significantly influence the position 
error obtained as long it is within the manipulators working 
volume. 

The ballbar method is ideal for calibration of industrial 
robots in that it is quick, inexpensive, and simple to 
perform. The manipulator can be calibrated in place without 
the use of expensive measurement and the ballbar calibration 
method can produce manipulator accuracy of the same order of 


magnitude as other more costly and tedious methods. 
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Figure 19. Position Error at x=1292.7 mm. 
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Figure 20. Position Error at x=1542.7 mm. 
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Figure 21. Position Error at x=1792.7 mn. 
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V. CONCLUSIONS 


In general, accuracy of parameter identification and 
manipulator accuracy increased as the number of 
observations taken increased. 


Accuracy of calibration process is directly related to the 
extent that measurement noise is reduced. 


Ballbar method produces manipulator accuracy of the same 
order as the unconstrained method and it is less 
expenSive, quicker, and easier to perform. 


The location of the ballbar does not significantly 


influence the accuracy of the calibration as long as it is 
within the robot’s working volume. 
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APPENDIX A: ZXSSQ 


ZXSSQ is a Levenberg-Marquardt finite difference routine 
for solving non-linear least squares. The problem can be 
SEaAbCOGmaSs tol lows: 

Given M non-linear functions F,, F,, ..., F, Of a veg=e=am 


Parameter x, minimize over x 


Fie) SE enemas se 


mM 
where xX = (X;, Xo, «+-+,Xy) 1S a vector of N parameters to be 
estimated. When fitting a nonlinear model to data, the 


functions F, should be defined as follows: 


Bea 2) S8y fe ee ae 7 = ee 


aa 
where YY, is the i" observation of the dependent variable 


Une eke Viv) is a vector conta. 
the i™® observation of the NV independent variables 


g is the function defining the non-linear model 
ZXSSQ 1s based on a modification of the Levenberg-Margquardt 
algorithm which eliminates the need for explicit derivatives. 
Let x° be an initial estimate of x. A sequence of 


approximations to the minimum point is generated by 


red = 5 | De oe) aa) 
where J, 1s the numerical Jacobian matrix evaluated at x® 
D, is a diagonal matrix equal to the diagonal of dea 


G@, 1S a pOSitive Scaling siaeren 
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When forward differences are used, the Jacobian is calculated 


by 


ay FF Cet yyy) Fy (0) 


Tiere UU, 1s the 3°" unit vector 


3 


ie nae 0.1) eps = 


eps is the relative precision of floating point 
aie HMer1 Cc 


For central difference, the Jacobian is as follows 





1 
OF, Loe eeu) ane: ae a) ] 
Finally, 
el aa [een ere Fri) Semon Oe 
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APPENDIX B: COMPUTER PROGRAMS 


Ce KKKK KK KKK Ko KR kkk Rk kkk kkk ke aK KK KKK KKK KK KK KKK KKK KK KK KKK KKK KKKKKKKK 


PROGRAM JOINT 


C This program generates the joint variable data for the 
C MODEL G manipulator simulation by random methods. 


INTEGER I, J, K, NOBS, MAXNOBS 
PARAMETER (MAXNOBS=360) 
REAL Q(MAXNOBS, 6), QOMIN(6), QMAX(6) 


COMMON /C1/ Q, QMAX, QOMIN 


© DATA QMIN/ -160.0, —223.0,—-52.0, =110-0, -100.0, -—Z6e6G 70m 

C DATA QMAX/ 160.0, 43.0, 2s2e07 3) 7 UO nO Cr Oe 660i 

Sc WRITE (6,*) ‘Volume is MAX-POSSIBLE’ 
DATA QOMIN/ -180.0, -180.0, 0.0, -180.0, -180.0, -180.0 / 
DATA QOMAX/ 180.0, 180.0, 76220, 9160207 EeUec 16070 

WRITE (6,*) 'Volume is FULL’ 

C DATA QMIN/ -90.0, -90.0, -90.0, -90.0, -90.0, -90.0 / 

& DATA OMAX/ 90.0, 90.0, 90.0, 902077 590-07 30-07, 

c WRITE (6,*) ‘Volume is HALF’ 

Ce DATA QMIN/ -45.0, -45.0, -45.0, -45.0, -45.0, -45.0 / 

G DATA OMAX/ 45.0, 45.0, 45.0, 45.0; 4570; 45720e, 

Ee WRITE (6,*) ‘Volume is QUARTER’ 

C Open Output satan fine 


OPEN (18, NAME=’ TELE-VAR.DAT’, STATUS=’ NEW’ ) 
C Input number of observations from data file 
OPEN (19, NAME=’ INPUT.DAT’, STATUS=’ OLD’ ) 
DO: f=d2170 
READ (19, *) 
ENDDO 
READ (19,*) NOBS 
WRITE (*, *) * NOBS= 7 Noes 
CLOSE (19) 
C Call the generation routine 
CALL MSPREAD (NOBS) 
C Save the joint variable data 


DOE — 1) INOBS 
WRITE (18,*) Q(TI,1),Q(1I,2),Q(II,3),Q(11,4),Q(II,5),Q(ITI, 6) 
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ENDDO 


CLOSE (18) 
SOP 
END 


ee KKK KKK KK KKK KKK KKK KKK KKK KKK KKK RK KKK KKK KKK KKK KKK KKK KKK KKK KKK KKK KKK K 
SUBROUTINE MSPREAD (NOBS) 
This subroutine generates the joint data by the Monte Carlo method. 


The six joint variables are generated from six independant 
uniform random variables. 


Om) 


PiTeGER 7) J, K, NOBS, MAXNOBS 
PARAMETER (MAXNOBS=360) 

REAL Q(MAXNOBS,6), QMIN(6), QMAX(6) 
INTEGER*4 ISEED 

REAL MAGQ (6) ,NUM 


COMMON /Cl1/ Q, QMAX, QMIN 
C Get the random seed 


WRITE (6,*) ‘Type in a 6-digit random number seed’ 
Renesas.) SEED 


Gecalculate the Scaling factor for each random variable 
DG I = i, 6 
MAGQ (I) = QMAX (I) -QMIN (I) 
ENDDO 


C Generate the joint data 


DO J 
pO 1 


Te NOBS 
eo 


CALL RANDOM (ISEED, NUM) 


QO(J,I) = QMIN(I) + MAGQ(I) * NUM 
ENDDO 
ENDDO 


RETURN 
END 


C KKK KKK KKK KKK KKK KKK KKK KKK KKK KKK KKK K KKK KAKA KKK KK KKK KK KK KK KK KK kk kkk 


SUBROUTINE RANDOM (x, 2Z) 


REA ieee xe Zi 
PNTEGER A, x, 1, M 
DATA I/1/ 


12 (LS BO2e0 ) GORTo 1000 
I=0 

M= 2 ** 20 

FM= M 

A= 2**10 + 3 
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1000 X= MOD( A*X ,M) 
FX= X 
Z= FX/ FM 


RETURN 
END 


ce KKK KEK KR KKK KKK KKK KEK KEK KKK KKK KKK KKK KKK KKK KKK KK KKK KKK KKK KEKE KKK 
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PROGRAM POSE 


C This program generates the pose data for the MODEL G manipulator 
C simulation. It reads the joint variable data from file TELE-VAR.DAT. 


INTEGER*4 ISEED 

REAL*8 RNX, RNY, RNZ, MAGNX, MAGNI1 
REAL*8 RN1, RN2, RN3, RN4, RNS, RN6 
INTEGER I, J, K, NOBS, MAXNOBS, N 
PARAMETER (MAXNOBS=360) 

REAL*8 DANGLE, DLENTH 

REAL*8 PI 

PARAMETER (PI=3.141592653589793) 


REAL*8 DTW, DT1, DT2, DT3, DT4, DTS 

REAL*8 DDW, DD1, DD2, DD3, DD4, DD5 

REAL*8 AAW, AAl, AA2, AA3, AA4, AAS 

REALS ATW. fil, AZ, AlL3, AL4, ALS 

REAL*8 BLW, BL1, BL2, BL3, BL4, BLS 

REAL*8 DF6, FI6, TH6, SI6, PX6, PY6, PZ6, D3 

REAL*8 THETA], THETA2, THETA3, THETA4, THETAS, THETAG 
REAL*8 THW, TH1, TH2, TH3, TH4, THS 

Rabe ese mw<4,4), T1(4,4), T2(4,4), 1T3(4,4) 

REAL*8 T4(4,4), 1T5(4,4), 1T6(4,4), TRPY(4,4), TXYZ(¢4, 4) 
REAL*8 TIMAT(4,4), 1T(4, 4) 


Seimacialiize the TIMAT matrix to an I matrix: 
arr TLIMAT/ Wor, 0,0,/1,0;,0,6,0,1,0,0, 0,0,1/ 
C Get the random number seed 


WRITE (6,*) ‘Type in a 6-digit random number seed’ 
READ (5,*) ISEED 


C Open input files and output data file 


OPEN (8, NAME='TELE-VAR.DAT’, STATUS=’OLD’ ) 
OPEN (9, NAME='TELE-POS.DAT’, STATUS=’NEW’ ) 
OPEN (10,NAME='INPUT.DAT’, STATUS=’OLD’) 


C Input parameters 


read (10,*) 

read (10,*) dtw,ddw, aaw,alw,blw 
mead (10e~). dtl,ddl,aal,all, bil 
mead (lOe*) dt2,dd2,aa2,al2,bl12Z 
meacg. (ORs) dts,dds,aa3,al3,bl3 
read (10,*) dt4,dd4,aa4,al4,b14 
mead (07) dt5,dd5,aa5,al5,b15 
read (10,*) 

medic (lO = jrecdio,cir6, S10, D6, Dy 6, Dz 6 
read (10,*) 

read (10,*) nobs,n, dangle, dlenth, magnx,magnl 


C Add encoder Offsets: 


DIW = DTW + DANGLE 
DT1 = DT1 + DANGLE 


ak 


DTZ =  DIZz + DANGLE 


Drs = Sbrs ! defined 
DT4 = DT4 + DANGLE 
DT5 = DT5 + DANGLE 


C Set link parameters for the manipulator: 


ALW = ALW + DANGLE 
AL1 = AL1 + DANGLE 
AL2Z = ALZ2 + DANGLE 
AL3 = AL3 + DANGLE 
AL4 = AL4 + DANGLE 
ALS = ALS + DANGLE 
AAW = AAW + DLENTH 
AAI] = AAl + DLENTH 
AAZ2 = AA2 + DLENTH 
AA3 = AA3 ' defined 
AA4 = AA4 + DLENTH 
AAS = AAS + DLENTH 
DDW = DDW + DLENTH 
DD1 = DDI + DLENTH 
DD2 = DD2Z2 + DLENTH 
DD3 = DD3 + DLENTH 
DD4 = DD4 ' defined 


DDS = DDS "+ DLENTH 


BLW = BLW ! defined 
BL1 = BLl ! defined 
BL2 = BL2 ! defined 
BL3 = BL3 + DANGLE 
BL4 = BL4 ! defined 
BLS = Bis ! defined 
DF6 = DF6 + DANGLE 
THO = 0.0 
S16 = 0.0 
PX6 = PX6 + DLENTH 
PY6 = 0.0 
PZ6 = PZ6 + DLENTH 
D3 = DD3 


C Loop NOBS times 
DO f=) 1) NeCBS 
C Initialize the T matrix to am I mMaeer 
DO J=1,4 
DO K=1,4 
T(J,K) = TIMAT(J,K) 


ENDDO 
ENDDO 


C Matiipulator joint angle siapuc 
READ (8,*) THETA1, THETA2, THETA3, THETA4, THETAS, THETA6 


THW = DIWw 
He Dial LHe ee I 


2 


TH2 = DIZ > + 
TH3 = DT3 
TH4 = DT4 + 
tho = oDTS + 
FI6 = DF6 + 
DD3 = D3 


THETAZ2 


THETA4 
THETAS 
THETA6 
+ THETA3 


C Compute the T matrices, 


DW thru G6: 


CALL TRANSFORM ( ALW, AAW, DDW, THW, BLW, 
CALL TRANSFORM ( AL1l, AA1l, DD1, TH1, BLl, Tl 
CALL TRANSFORM ( AL2, AA2, DD2, TH2, BL2, T2 
Grp TRANSEORM ( ALS, AAS, DD3, TH3,5 BL3, T3 
CALL TRANSFORM ( AL4, AA4, DD4, TH4, BL4, T4 
CAD ERANSEORM ( ALS; AAS, DDS, THS, BLS, T5 
CV oleEsoRey wGlrro, THE, SIG, °TREY) 

Chiao YZ 6 PXG6, PY625PZ6, TXYZ ) 
CAL MATMULC ( T6, TREY, TXYZ ) 

C Compute the overall transformation, T: 

Sl MATMULA ( T, TW ) 

C2 MATMULA (°T, Tl ) 

Ail MATMULA {( T, T2 ) 

CALL MATMULA ( T, T3 ) 

CALL MATMULA ( T, T4 ) 

CALL MATMULA ( T, T5 ) 

CALL MATMULA ( T, T6 ) 


C Generate the random noise 


CALL 
CALL 
CALL 


RANDOM (ISEED, RNX) 
RANDOM (ISEED, RNY) 
RANDOM (ISEED, RNZ) 


CALL 
CALL 
CALL 
CALL 
CALL 
CALL 


RANDOM (ISEED, RN1) 
RANDOM (ISEED, RN2) 
RANDOM (ISEED, RN3) 
RANDOM (ISEED, RN4) 
RANDOM (ISEED, RNS) 
RANDOM (ISEED, RN6) 


RNX = MAGNX* ( 2.0*RNX 
MAGN ( Z2.0*RNY = 1. 
MAGNX* (7 2-0*RNZ — 1. 


RNY 
RNZ 


RN1 
RN2 
RN3 
RN4 
RN5 
RN6 


MAGN 1 * ( 
MAGN 1 * ( 
MAGN 1 * ( 
MAGN1 * ( 
MAGN 1 * ( 
MAGN 1 * ( 


70 Rnd = 
BOC Ne 
OSI os 
oN 
,0*RNS = 
nO RNG = 


Por he ND NN 
PRP PPP 
BSS O OO 


C Add noise to measurements and encoder readings 


T{1,4) = T(1,4) + RNX 
Tie 4je= £2, 4) + RNY 
T(3,4) = 7(3,4) + RNZ 
THETA] = THETA] +RN1 
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THETA2 — THE TAZSaRNZ 


THETA3S = THETASea2N3 
THETA4 = THETA4 +RN4 
THETAS = THEDTASe a> 


THETA6 = THETA6 +RN6 
C Store the manipulator joint vector and measured tool pose 


WRITE (9,991) THETA], THETA2, THETA3, THETA4, THETAS, THETAS6 
WRITE (9,992) T(1,4) 

WRITE (9,992) T(2,4) 

WRITE (9,992) T(3, 4) 

WRITE (9, *) 


C Format below decides the digits Of accuracy of simulatwvon data 


991 FORMAT ( 6F12.6 )!Joint vector data 
992 FORMAT ( F12.5 ) 'Measurement data 


C Bnd do=lo0op for countewe. 
ENDDO 
WRITE (6,%*) ’Data stored in F12.5, FIZ. 43teenae 


CUCGSE (a) 
CLOSE G9} 
SLOP 
END 


G KARR KKK KK KKK KKK KKK KKK KKK KKK KKK KKK KAR KKK KK KKK KKK KKK KKK KKK KAKA KKK KK 


SUBROUTINE RANDOM (x, z) 


READE Meee 
INTEGER A xX, Lp oM 
DATA 1/17 


TreGel, wee Oo Gen TO 1000 
I=0 

M= 2 ** 20 

FM= M 

A= 2 eS 


L000 X= MOD( A*xX ,M) 
Eo 
Z= FX/ FM 


RETURN 
END 


GC KKKKKKKKKKEKKKKKKKKKKKKKAKKRKKKKKKEKKAEKKKKKKKKEK EKER KRKKKKEKKKKKKKKXEK XK 
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PROGRAM TELEBAR 


This program generates a set of joint angles for the calibration 
of the MODEL G manipulator using a ball bar to constrain the end 
point of the manipulator. 


CHO 


INTEGER LDFJAC, M, N, obs, nobs 
PARAMETER (LDFJAC=3, M=LDFJAC, N=6) 


REA oe DT bie, prs, DT4, DIS 

REAL*8 DD1, DD2, DD3, DD4, DD5 

REAL*8 AAl, AA2, AA3, AA4, AAS 

REA 8 Als AL?, ALS, AL4, ALS 

REAL*8 BL1, BL2, BL3, BL4, BL5 

REAL*8 DF6, FI6, TH6, SI6, PX6, PY6, PZ6 
REAL*8 XW, YW, ZW 


DWEGHR tmfer,1er,10opt,nsig,maxin 

Pei swAe (MDE TAC, N), x jt} ( (n+l) *n/2Z2), xjac (ldfjac,n) 
Pore ompearm (4) 7 t (idfjac), work (5*n)+(2*m) +( (nt+1)4n/Z) ) 
REAL*8 X(N) 

real*8 r,phimax, phimin, thetamax,thetamin, phi,theta 
Bead! ~ Gx, YO, Z2D,SSG, rr, magqnx,magnl 


EXTERNAL TELE ARM 


INTEGER I, J, K 

REAL*8 TDES (4,4), qmax(6), qmin(6), SCALE, DANGLE, DLENTH, NUM 
COMMON /PDATA/ TDES, DANGLE, DLENTH, r 

ZeMMON /KIN/ DT1,DT2,DT3,DT4,DT5, 

& Aiea 2 Alea: ALS, 

AA1, AA2, AA3,AA4,AA5, 

Day DZ DDS, PD4- DDS, 

Bil, el2 Sle, 84, 8L5, 

XW, YW, ZW, 

DF6, TH6,S16,PX6,PY6, PZ6 


RY RI RI RI Rr 


C Joint angle ranges 


aetearvamin/—30.0,-45.0,0.0,-180.0,0.0,-180.0/ 
@eaeaegman/Z2Z5.0, 45.0, 762.0, 180.0, 90.0, 180.0/ 


Meet ralize data variables 
obs=0 
C Open data files for input 


SEEN lO NAME— TELE-SOLN-DAT’, STATUS=’ NEW’ ) 
open (9, NAME=’INPUT.DAT’, STATUS='old’) 


C Read input kinematic data 


read (9,*) 

Bead) (9, =). 2w, Vw, ew 

Beade0l, -jmcel, OO 1 .aai,ali, 5b) 1 
read (9, *) 7atZ,dd2, aaZ,al2;b12 
Bead, «joes ,dds,aas,al3,b13 
read (9,*) dt4,dd4,aa4,al14,b14 


DO 


read (97 dts dds, ado, alo ,bole 

read (9,*) 

read (97 *)) at opto, s16, PxX6, DYo, pz2e 

read (9,*) 

read (9,*) nobs,r,dangle,dlenth,magnx,magnl 


close (9) 
C Adjust nominal values 


xw=xwtdlenth 
yw=ywtdlenth 


dt2=dt2+dangle 
dt 4=dt 4t+dangle 
dt 5=dt5+dangle 


all=ali+dangle 
al2=al2+dangle 
al3=al3+dangle 
al4=al4+dangle 
al5=al5+dangle 


aal=aal+dlenth 
aaZ=aazZtdlenth 
aa4=aa4t+dlenth 
aa5S=aadt+tdlenth 
ddi=ddit+dlenth 
dd2=dd2+dlenth 
dd3=dd3+dlenth 
ddd5=dd5+dlenth 
b1l3=bl3+dangle 
df6e=df6+tdangle 
pxo=px6t+dlenth 
pz6=pz6tdlenth 

C Limits on bar rotation 
phimax=180.0 
phimin=-180.0 
thetamax=90.0 
thetamin=-90.0 

C Get random number seed 


Cc ISEED = 123456 


write (6,*) ‘Type in a 6-digit random number seed’ 
read (5,*) iseed 


C Write NOBS to TELE-SOLN.DAT 
write (10,*) nobs 
C Startcote main, loop 


O50 obs=obstl 
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C Set joint angles to zero 


Go 1=15n 
x(1i)=0.0 
enddo 


C Get random bar angles 


1000 call random (iseed, num) 
phi=phimin+ (phimax-phimin) *num 
call random (iseed,num) 
theta=thetamin+ (thetamax-thetamin) *num 


Secalculate end point of the bar 


xb=r*cosd (theta) 
yb=r*sind (theta) *cosd (phi) 
zb=r*sind(theta) *sind (phi) 


C Reacheability calculation 


if (z .1t. 0.0) go to 1000 


C Establish desired tool pose 


do ii=1,4 
do jj=1,4 
DES (21, 7))=0.0 
enddo 
enddo 


TDES (1, 4) =xb 
TDES (2, 4) =yb 
mbes (3,4) =zb 
moeES(4,4) = 1.0 
C Call IMSL ZXSSQ for inverse kinematic solution 
nsig=4 
eps=0.0 
delta=0.0 
maxfn=500 
L1opt=1 
ixjac=ldfjac 
CALL ZXSSQ(tele arm,m,n,nsig,eps,delta,maxfn, iopt, parm, x, 
& Ssca, §-*jJec, 41s jacpxjt)jpwOLrk, infer, 1er) 


C Print results to 2 decimal places 


write(6,*) obs,ssq, iseed 
PinieurmenOme exC lin xX (2) (3), X14), *%(5) 4° X06) 


C Continue for other bar angles 
Mamoes 2 1t. mobs) go to 1010 


GEOSE 410) 


a 


WRITE (6,*) XW, YW, ZW 
END 


C KKK KKK KKK KKK KKK KKK KKK KKK KKK KKK KKK KKK KKK KKK KKK KKK KKK KKK KKKKKKKKKKKKKKK 


SUBROUTINE tele ARM (X,M,N,F) 


a. 


the MODEL G manipulator. 


INTEGER M, N 
REAL*8 X(N), F (M) 


TN LEGER abr mod 

REAL*8 DT1, DT2, DT3, DT4, DTS 

REAL*8 DD1, DD2, DD3, DD4, DDS 

REAL*8 AA1, AA2, AA3, AA4, AAS 

REAL*8 AL1, AL2, AL3, AL4, ALS 

REAL*8 BL1, BL2, BL3, BL4, BL5 

REAL*8 DF6, FI6, TH6, SI6, PX6, PY6, PZ6 
REAL*8 XW, YW, ZW, D3 


REA Oo Hh, the.) Ho, eho eS 

REAL*8 TQ (4,4), T1(4,4), T2(4,4), TS (Qegape 12 (are 
REAL*8 T5(4,4), T6(4,4), trpy(4,4), txyz(4, 4) 
REAL*8 TIMAT(4,4), T(4, 4) 

REAL*8 disq,dis 


INTEGER Wee kK 
REAL*8 TDES (4,4), DANGLE, DLENTH, r 


COMMON /PDATA/ TDES, DANGLE, DLENTH, r 

COMMON /KIN/ DT1,/DT2- Dis DIidabi>, 
AL1, AL2, AL3, AL4,AL5, 
AA1,AA2,AA3,AA4,AA5, 
DD1, DBZ, DD3,,DD4.,.6 b>, 
BL1,Bu2Z, BLS, SL4, elo, 
XW, YW, ZW, 
DF6, TH6, S16, PX6, PY6, PZ6 


MRM RRR mR 


C Initialize the TIMAT matrix to an © iiaeL 
DATA STIAMAT/ 1.0, 0:0,0,1,0,070, O77 40510 Wore, 
© Inatiabrzesthes. Matric Lowder. 
DO II = 1,4 
DO JJ ="1,-4 
Pipe Jd) = TMA. ip 


ENDDO 
ENDDO 


C Manipulator joint angles 


THi— DIL + K(1) 
TH2e—w bi + XZ) 


TH3 = DT3 

TH4°=> Dre + X14) 
THS = D5-+. X15) 
FI6 = DF6 + X(6) 


oO 


This subroutine calculates the non-linear Eunction tor the mse eam 
the IMSL routine ZXSSO. It is the forward kKinematiwve solution tem 


Pee poo sea4 X (3) 
@mcompute the £ matrices, Tl thru T6: 
CALEatoxy2 (XW, yw, zw, 10) 


CALL TRANSFORM 
CALL TRANSFORM 
CALL TRANSFORM 
CALL TRANSFORM 
CALL TRANSFORM 


PolewAn |: DDI o THI, Bul, T1 5 
ieee? DD. Thee BLO, T2 ) 
Alor ennS =. DS. Dhow bbo;. Lo) 
hia, 0AA4, DDIae tee sEu4, T4 ) 
ALS, AAS, DDS, THS, BLS, T5 ) 


A a 


CAbimeoroy ( £16,°th6, Si6, trpy ) 
GrisgwtaxyZ ({ PX6, PY6, PZ6, txyz ) 
Gash matmulc (to, trpy, txyz ) 


C Compute the overall transformation, T: 


CALL MATMULA ( T, TO ) 
CALL MATMULA ( T, Tl ) 
CALL MATMULA ( T, T2 ) 
CALL MATMULA ( T, T3 ) 
CALL MATMULA ( T, T4 ) 
CALL MATMULA ( T, TS ) 
CALL MATMULA ( T, T6 ) 


GwGalculate the function F 


£ (1) =t (1, 4) -tdes(1, 4) 
f (2) =t (2, 4) -tdes (2, 4) 
£(3)=t (3, 4) -tdes (3, 4) 


RETURN 
END 


cS KKK KKK KKK KKKKK KKK KK KKK KK KKK KKK KKK KKK KKK KK KKK KKK KKK KKK KK KKKKKKKKKKKKKK 


SUBROUTINE RANDOM (x, 2) 


C This subroutine generates random numbers in the range 0-1 
C using a supplied seed x, the returned random number being z. 


REAL FM, FX, Z 
INTEGER A, X, I, M 
DATA i 1/ 


tee ote tO. 0) CGOrTO 1000 
I=0 

M=32 2 0 

FM= M 

A= 2**10 + 3 


1000 X= MOD( A*X ,M) 
FX= X 
Ze eer Mi 


RETURN 
END 


Cc KEKE KK KKK KKK KKK KKK KKK KKK KKK KKK KKK KK KKK KKK KKK KK KKK KK KKK KKK KKK KKK 
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C KRKKKKKKKK KKK KKK KKK KKK KKK KK KK KKK KKK KKK KKK KKK KAKKKA KKK KKK KKK KKK KKK KK KKK 


PROGRAM ID6 


Robot Identification using the Non-linear Least Squares method. 
Simulation data is read for the MODEL G manipulator from 
the data file TELE-SOLN.DAT 


Change parameter LDFJAC to change the number of observations, 
set LDFJAC = Number of observations 


CHG) eC) COG2 


INTEGER LDFJAC, MM, M, NN, N, NS1IG, MAXFN, IOPT, IXJAC, INFERe. 
PARAMETER (LDFJAC=90, MM=LDFJAC, NN=22) 


REAL*8 FJAC(LDFJAC,NN), XdJTJ((NN+1) *NN/2) 

REAL*8 PARM(4), F(LDFJAC), WORK((5*NN) +(2*MM) +((NN+1) *NN/2) ) 
REAL*8 X(NN) 

EXTERNAL TELE ARM 


REAL*8 DANGLE, DLENTH, TQ, DQ, EPS, DELTA, SSQ 
REAL*8 SOERR1, SQERR2 

REAL*8 XW, YW, ZW 

REAL* © Dll, bre, Dis a4) eS 

REAL*8 DDl,. Db2 a DDo Dh 4, DDS 

REAL*8 AAl, AA2, AA3, AA4, AAS 

REA 0 All AL 2 eee A een 

REAL*8 BL1, BL2, BL3, BL4, BLS 

REAL*8 FI6, DF6, TH6, SI6, PX6, PY6, PZ6 


INTEGERS. 0, h, NOOBS, MAXNOEBS 

REAL*8 magnx,magnl 

PARAMETER (MAXNOBS=100) 

REAL*8 TET1(MAXNOBS), TET2(MAXNOBS), TET3 (MAXNOBS) 

REAL*8 TET4 (MAXNOBS), TET5(MAXNOBS), TET6 (MAXNOBS) 

REAL*8 R 

COMMON /PDATA/ NOBS, TST1, TET2Z, TET3, TET4, tet leloye. 


COMMON /KIN/ DTI1l7DTZ2, Dis, Dia, pio 

& Al pee ee oe, AT Oy 
AA1,AA2,AA3,AA4,AAS, 
DDI, PDZ,DD3 DD4,Dbor 
Bol -BL2, BLS, BL4, BES; 
XW, YW, ZW, 
DF6,TH6, S16,PX6,PY6, P26 


— Ry Ry Ry — 


C Open data files for inputs and results 


OPEN (8, NAME=’ RESULT.DAT’, STATUS=’ NEW’ ) 
OPEN (9, NAME=’TELE-SOLN.DAT’, STATUS=’ OLD’ ) 
OPEN (10,NAME=’ INPUT.DAT’, STATUS=’ OLD’ ) 


c Read input parameters 


read (10, *) 

read (10,*) xw, yw, zw 

readme, =) Tt ll adderaal alle 
reéaq ({10,*) dt2,dd2,aaz,alZ,boi2Z 
réaqgn(10,*)> .dt3,dd3aa3 pale, b's 
read (10,*) dt4,dd4,aa4,al4,b14 
rea A107-*)) atS,;dd5,aa5,a15 21> 
read (10,*) 
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Peaqcumulon =) cdr 6,tno,s10,Pxo, py >o,pz0 
read (10, *) 
Treadmill, =) Mobs, 1, dangle, dlenth,magnx, magnl 


CUOSE: (80) 
C Initialize data variables 


X (1) =XW 
X (2) =YW 


5) —=DD1 
X (4) =AA1 
X (5) =AL1 


mo) —DTZ 
X (7) =DD2 
X (8) =AA2 
X (9) =AL2 


X (10) =DD3 
X (11) =AL3 
X (12) =BL3 


X(13) =DT4 
Xx (14) =AA4 
X (15) =AL4 


m(16)—=DT5 
X (17) =DD5 
X (18) =AA5 
X (19) =AL5 


x20) =DEF6 
X (21) =PX6 
X (22) =PZ6 


R=R+MAGNX 
C Read simulated joint data and tool pose 
READ (9,*) NOBS 


DO J = 1, NOBS 

READ (9,*) TET1(J), TET2(J), TET3(J), TET4(J), TETS(J), TET6(J) 
ENDDO 
CLOSE (9) 


@eecalt IMSL routine for non-linear identification 


NS IG=4 

BE O—O. 0 
DELTA=0 .0 
MAXFN=1500 
IOPT=1 
IXJAC=LDF JAC 
M-=NOBS 


CALL ZXSSQ(TELE_ARM,M,NN,NSIG, EPS, DELTA, MAXFN, IOPT, 
& PARM, X,SSQ,F,FJAC, IXJAC, XJTJ, WORK, INFER, IER) 
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C Save results to data file 


WRITE (8, *) 

WRITE (8,*) ‘XW, YW, ZW’ 

WRITE (8. *) x (1p eet) ZW 

WRITE (8, *) 

WRITE (8,*) ‘DT1, DD1, AA1, ALi, Bul’ 

WRITE (8,%)) 0.0, x03), X (4). 5 eeOnO 
WRITE (8, *) 

WRITE (8,*) “DTZ, DD2, AA2, AZ) BLZ’ 

WRITE” (8, *) 9X (6), X17), X (8) oO 
WRITE (8, *) 

WRITE (8,*) °DT3, DD3, AA3, Auopeeuee 

WRITE (8,*) @.0, X(10), 020,78 <U pee) 
WRITE (8, *) 

WRITE (8,*) ‘DT4, DD4, AA4, AL4, BL4! 

WRETE (8,*) X(13);, 0.0, X(14)) xtoeeo 0 
WRITE (8, *) 

WRITE (8,*) ‘'DTS5, DDS, AAS, ALS, BLS’ 

WRITE (8,*) X(16),%(17), X(18), xX (1SipOnc 
WRITE (8, *) 

WRITE (8,*) ' DF6, TH6, SI6, PX6, PY6, PZ6, R’ 
WRITE (8, *) “X(20)- O20e, 020, x (21). OO ae ree 


c Restore initial values of input parameters 


open (10,name=’input.dat’,status=’old’ ) 
read (10, *) 
read “10, *) xw, yw; zw 
read. (0; ep atl ddl. aalvatll, bit 
read SOO  wrdt2Z,ddZ,aaZ2,alz,blZ 
read “QUO wdts, dd3s;dat,a! 37515 
read (10,*) dt4,dd4,aa4,al4,b14 
read (1074 )'tat >, daS7aaS,al57o15 
read (10, *) 
read (C10, *) dit, tic, sic, peo, ovo, pzo 
reads (0... } 
read (10,*) nobs,r,dangle,dlenth,magnx,magnl 
CLOSE (10) 


C Calculate root mean square error in identification 


TQ = DANGLE 
DQ = DLENTH 
C Error in identification (angular parameters) 


SQERR1 = 


(ALI4+TO=-X(5)) **2 +(DT2Z+TO=x (6) ) **2 es (9) 


& 

& “+ (Aiss 1 O-x (1) ) eZ 

& (pis Ox (12) jes = 7 
& t(ALS+EO—X (15) ) **2 2(DTSst2O-x( ie) a 
& Tae LO (100 eZ 
&  +({DFotTO=xX(20)) **Z2 

SQERR1 = DSQRT( SQERR1/10 ) 


C Error in identification (length parameters) 


SQERR2 = 
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+(DT447O—x (13) 


OO) 


& (DD1+DQ-X (3) ) **2 +(AA1+DQ-X (4) ) **2 
& + (DD2+DQ-X (7) ) **2 +(AA2+DQ-X (8) ) **2 

& + (DD3+DQ-X(10))**2 +(AA44+DQ-X (14) ) **2 
& + (DD5+DQ-X (17) ) **2 +(AA5+DQO-X (18) ) **2 
& + (PX64+DQ-X (21))**2 +(PZ6+DQ-X (22) ) **2 
Cet (xwrag—-x (1) )**Z qa 1 CG xe) 
SQERR2 = DSORT( SQERR2/12 ) 


WRITE (8,*) 

WRITE (8,*) ‘RMS PARMS (LENGTH), RMS PARMS (ANGLE) ’ 
WRITE (8,*) SQERR2, SQERRI1 

WRITE (6,*) ‘RMS PARMS (LENGTH), RMS PARMS (ANGLE) ’ 
WRITE (6,%*) SQERR2,SQERR1 


WRITE (8, *) 
WRITE (8,*) ‘INFER, IER,NOBS,NSIG’ 
WRITE (8,*) INFER, IER,NOBS,NSIG 
WRITE (6,*) ‘INFER, IER,NOBS,NSIG’ 
WRITE (6,*) INFER, IER,NOBS,NSIG 
WRITE (8,*) 


SEOSE (3) 


END 


KKKKKKKKKKK KKK KKK KKK KKK KKK KKK KKK KKK KKK KKK KK KK KKK KKK KKK KKK KKKKKKKKKKKK 


SUBROUTINE TELE ARM (X, M, N, F) 


This subroutine calculates the non-linear function for the use of 
the IMSL routine ZXSSQ. It is the forward kinematic solution for 
the MODEL G manipulator. 


INTEGER M, N 
REAL*8 X(N), F(M) 


CECE R Il, JJ 


REAL*8 XW, YW, ZW 

REAL*8 DT1, DT2, DT3, DT4, DTS 

REAL*8 DD1, DD2, DD3, DD4, DDS5 

REAL*8 AA1, AA2, AA3, AA4, AAS 

Rem seat AL?, Abs, AL4, ALS 

REAL*8 BL1, BL2, BL3, BL4, BL5 

ReAbwoerilG, G£6, th6, si6, PX6, PY6, PZ6, D3 


REAL*8 TH1, TH2, TH3, TH4, THS5 

REAL*8 T0(4,4), 1T1(4,4), 172(4,4), 173(4,4), 7T4(4, 4) 
REAL*8 T5(4,4), T6(4,4), trpy(4,4), txyz (4,4) 
REAL*8 TIMAT(4,4), 1T(4, 4) 


INTEGER I, J, K, NOBS, MAXNOBS 

PARAMETER (MAXNOBS=100) 

REAL*8 TET1(MAXNOBS), TET2(MAXNOBS), TET3 (MAXNOBS) 

REAL*8 TET4(MAXNOBS), TET5 (MAXNOBS), TET6 (MAXNOBS) 

REAL*8 R, RR 

COMMON PhAUn/ NOES) TETI, TETZ, TETS, TET4, TETS, TET6, R 


GOMMON A KIN, Dil, DIZ, DITS,DI4,DT5, 
& Pigiewnit? his Abd Ais S, 
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AA1, AA2,AA3,AA4,AA5, 
DD1,DD2,DD3,Db4,DD5, 
BL1, BL2, BL3, BL4, BLS, 
XW, YW, ZW, 

DF6, TH6, SI6, PX6, PY6, PZ6 


RR RR km 


C Initialize the TIMAT matrix t6 am ema. - 
DATA TIMAT/ 1, 0,0.-0,0,.4)10,.0,0 07-1) OrecrOr Oe ky 


C Set parameters for the manipulator: 


AW = XXL) 
YW = X(2) 
DD =" X03) 
AAl = X(4) 
AL1 = X(5) 
DT2 = X(6) 
DD2 = X(7) 
AA2 = X(8) 
AL2 = X(9) 
DD3 = X(10) 
AL3 = X(11) 
BL3 = X(12) 
DT4 = X(13) 
AA4 = X(14) 
AL4 = X(15) 
DT5 = X(16) 
DDS?) >= X17) 
AA5 = X(18) 
ALS = X(19) 
DF6 = X(20) 
PX6 = X(21) 
PZ6 = X(22) 


C Loop NOBS times 


K = 0 
DO Ud sel, 7 NOOBS 


G Initialize the T matrix covanme! mace 


4 


DO II =1, 
po JJ = 1,4 
Tile) e= TIMAT lion 
ENDDO 
ENDDO 


C Manipulator joint angles 


Tha Dis Ler i) 
TH2)= "Daze TEhiZ (J) 
THS = bas 

TH4 = DT4 + TET4 (J) 
ToS piss) PETS (J) 
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B16 °—" DEG + TET6(J) 
Pee DUS.t IlETS()) 


C Compute the T matrices, Tl thru T6: 


CALL T3XYZ (XW, YW, ZW, TO) 
CALL TRANSFORM ( ALI, AA], DDI, TH, BLl, Til ) 
GALL TRANSEORMa(wALZ2, AAZ, DDZ, TH2Z, BLZ, T2 ) 
Cmll TRANSPORM ( Als, AAS, D3, TH3, BL3, T3 ) 
CALL TRANSFORM ( AL4, AA4, DD4, TH4, BL4, T4 ) 
cab TRANSE ORM { ALS, AAS, DD5, TH5S, BLS, TS) 
ernie tgrepy ( £36, tho, sit, trpy ) 
ern T3xkvZ ( PXG, PYG, PZ6, tCxyz ) 
Seulematmule ( £6, troy, txyzZ ) 

C Compute the overall transformation, T: 
CALL MATMULA ( T, TO ) 
CALL MATMULA ( T, Tl ) 
GALL MATMULA ( T, T2 ) 
Shir MATMULA ( T, T3 ) 
CALL MATMULA ( T, T4 ) 
Crhu MATMULA ( T, TS ) 
CALI) MATMULA ( T, T6 ) 


@eealculate the function F 


me—asort( t (1,4)*t{1,4) +b (2,4) *t (2, 4) +t (3, 4) *t (3,4) ) 
fit) —dabs( rr—-r) 


Seeaa the do-loop for counter J 
ENDDO 
C Compute RMS error 


sumsq=0 .0 


do j=l, nobs 
Sumsc—sumsqtf ()) *f (7) 
enddo 

rms=sqrt (sumsq/nobs) 
write (6,*) rms 
RETURN 

END 


C KRKKKKKKKKKK KKK KK KKK KAKA KK KKK KR KKK kk kk kok kk kok ok kk kkk kk ok ok kk ok kk kkk kk ok ok 


GO 


Cc KKK KKK KKK KKK KKK KKK KKK KKK KKK KK KKK KKK KKK KKK KKK KEK KKK KKK KKK KKK KKK KKKKKKK 


PROGRAM VERES 


C This program generates the six-dof pose error for the MODEL G 
manipulator. 

C It contains the identified calibration parameters and the exact 
Paramecer= 

C It uses a data file of verification joint angle sets POSEVER.DAT, and 
the 

C file RESULT.DAT from the program ID6. 


INTEGER I, J, K, NPOSES, N 

REAL*8 DANGLE, DLENTH 

REAL*8 P(200),0R(200) ,W1 (200)  W2(200 Fs Z00} 

REAL*8 Dr(S),dd(5),aa(S),al(5)] oer vor «3) 

REAL*8 eDT(5),edd(5),eaa(5),eal(5),eb1(5), eworld(3) 
REAL*8 edf6, EFI6, ETH6, ESI6, EPX6, EPYG, EPZ6 
REAL*8 THETA(1000,6), TDELTA(4, 4) 

RBAL*8 T0(4,4). T1(4,4), T1214, 4.45 2347) 

REAL*8 T4(4,4), T5(4,4), 1T6(4,4), TREY (47 Are lorZ (4s) 
REAL*8 TIMAT (4,4), T(4,4), et(4, 4) 


REAL*8 DT1, DT2, DT3, DT4, DTS 

REAL*8 DD1, DD2, DD3, DD4, DD5 

REAL*8 AAl, AA2, AA3, AA4, AAS 

REARS OSA eal2,. ALS, Ald4> eas 

REAL*8 BL1, BL2, BL3, BL4, BLS 

REAL*8 DF6, FI6, TH6, SI6, PX6, PY6, PZ6 
REAL*8 XW, YW, ZW 

COMMON TIMAT, THETA 


C Initialize the TIMAT matrix Lo am 0 atrie 
DATA TIMAR/ 7-050 70 .0 al 0520) OO ale O OR Orden) 
C Open data file 


OPEN (9, NAME='’posever.DAT’, STATUS=’ OLD’ ) 
OPEN (10, NAME=’input.DAT’, STATUS=’OLD’) 
OPEN (11, NAME='result.DAT’, STATUS=’ OLD’) 


c Read input parameters 


read (iU7~) 

read (10,*) mrorld(t) world ewonld Ge) 
read (10, *)- dtl,ddlaai,4ai1, ci 

read (10,*) dt2Z2,ddZ aaZ,alz er 
read>-(10, *) dt3,dd3raas, alee. 

read (10,*) dt4,dd4,aa4,a14,bl4 

bread -(10,.*)- dvaraddS,aao,alone.o 

read (10, *) 

read (10;*) d£6,tho7si6G,px6, pyc, p25 
read (10, *) 

read (10,*) nobs,xr,dangle,dlenth, magus mom 


CLOSE (10) 


c Read in joint angle sets for verification poses 
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read (9,*) nposes 


do i=l,nposes 

Peace peheta (i, | )yetheta (1,2), theta (i,3),theta(i,4), 
& theta(1, 5), theta (1, 6) 

enddo 

close (9) 


C Set exact link parameters for the manipulator: 


ae(l) = dtl ! defined 
dt(Z) = dt2 + dangle 
dt(3) = dt3 ! defined 
at(4) = dt4 + dangle 
dat (5) = dt5 + dangle 
Worlat(l) = world(1) + dlenth 
Wwomlba(Z)) = world(Z) + dlenth 
al(1l) = all + DANGLE 
al(2) = al2 + DANGLE 
al(3) = al3 + DANGLE 
al(4) = al4 + DANGLE 
al(5) = al5 + DANGLE 
AA(1) = aal + DLENTH 
AA(2) = aa2 + DLENTH 
AA(3) = aa3 ! defined 
AA(4) = aa4 + DLENTH 
AA(5) = aa5 + DLENTH 
wel) = ddl + DLENTH 
BZ) = ddZ2 + DLENTH 
Bois) = dds + DLENTH 
DD(4) = dd4 ! defined 
BRio)e— aa. + DLENTH 
BL(1) = bll ' defined 
BuC2) = biz ! defined 
BL(3) = b1l3 + DANGLE 
BL(4) = bl14 ' defined 
BL(5) = bl14 ' defined 
DF6 = DF6 + Dangle 

TH6 = 0.0 

Sio7—= (0.0 
PX6 = PX6 + DLENTH 

PY6 = 0.0 


PZ6 = PZ6 + DLENTH 
c Read in and set up estimated parameter table 


meag (11, ~) 
read(1l1,*) 
read(11,*) eworld(1),eworld(2),eworld(3) 
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read (11,*) 

reagdel i.) 

read (11,*) edt(i),edd(i1),eaa(i),eal(i),ebl (1) 
enddo 


Gy, 


00 


O50 - G02) 


reacd(lai, *) 
read (1 ly*) 
read(ll,*) edis, Gent,es150,epxo,epyc,cezo,2 


do kk=1,3 
write (6, *)world (kk) ,eworld (kk) 
enddo 


do ii=1,6 

Write (Go) saa 

write (6, *)al(ii),eal(ii),aa (ai) eagani) dd (11) ecdeaiaa 
& biG), ebviiwyedtiad) Fede tam 

enddo 


Main loop through NPOSES joint angle sets 
do k=1,nposes 


call fks (k,world,dt,al,aa,dd,bl, £16, tno, suse 6, yo, O25, ce) 
call fks (k,eworld,edt,eal,eaa,edd,ebl,efi6,eth6,esit,epx6, 
& epy6,epz6,et) 


Compute the differential tool matrix 
call matsub (tdelta,t,et) 
Compute the pose errors 


poserr=sqrt (tdelta (1, 4) **2+tdelta (Z, 4) **2Z+tdeltags7 4) 2) 
orerril=(tdelta(3,2)—-tdeltatZ, sij72Z 

OrerrZ—(toeltatl 2i=-tdeltats ley 2 
orerr3=(tdelta(2,1)-tdelta(1,2))/2 

orerr=sqrt (orerrl**2+torerr2**2+orerr3**2) 


Update total error counts 


posterr=(poserrt+ (k-1) *posterr) /k 
orterr =(orerr +(k=1)*orterr)/k 


EnG Of Main loop 
enddo 


write (6,*) "Position eError, Orientation error 
write (6,*) posterr,orterr 


OPEN (19, NAME=’VER.DAT’, STATUS=’OLD’ ) 

READ (19,*) NR 

IF (NR.GT.0) READ(19,*) (P(ITVVOR(1) ,With), We (1) ,W3(1), 1-1, 
NR=NR+1 

p (nr) =POSTERR 

or (nr) =ORTERR 

wl (nr) =WORLD (1) -DLENTH 

w2 (nr) =WORLD (2) -DLENTH 

w3 (nr) =WORLD (3) 

REWIND 19 

WRITE(19,*) nr 

WRITE(19,*) (PQ), OR(1) Wi G) W221) wet), 1-1, NR) 
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CL@se (19) 


end 


Cc meen Aa K RAK KAR K KKK KK KKK KKK KK RRR AK RR RRR KR KK KK KKK KKK KKKKKKKEKKKEKKEEKKKK 


Subpsourime fees (nawerwo,dt,al,aa,ad,b1,da£6,th6,si6, 
& DxGarW eb Z0, tL) 


Remvignee | O(a. 4). TV(4 4), T2(4,4),.-1T3 (4, 4) 

Reet oer (4, 4)8 15 (4,4), T6(4,4), TREY(4,4), TAYZ (4,4) 
Rope omPiMAT (4-497) 1(4,4), dt (S),al(5),aa(5s),dd(5),b1(5) 
meal oo theta (1000, 6), ang(5), world(3) 

common timat,theta 


Meinmrtialize the T matrix to an I matrix: 


DO J=1,4 
DO K=1,4 

T(J,K) = TIMAT(J,K) 
ENDDO 
ENDDO 


C Set up the joint angles 
do i=1,5 


ang(i)=theta(n,i) 
enddo 


fi6=theta(n, 6) +df6 

Seeompute the T matrices, Tl thru T6: 
eee oxy2 (worild(1),world(2), world(3),TO0) 
Sra TRANSFORM (al(l),aa(l1),dd(1),ang(1),b1(1),T1) 
SemGetRANSFORM (al(2),aa(2),dd(2),ang(2),bl(2),T2Z) 
SeinibetRANSPORM (al(3),aa(3),ang(3),dt(3),b1(3),T3) 
CALL TRANSFORM (a1(4),aa(4),dd(4),ang(4),b1(4),T4) 
et TRANSFORM (al (S)7aa(5),dd(S),ang(5),b1(5),TS5) 
CALL T3RPY (fi6,th6,si6,TRPY ) 
Soils s (PxX6, Py0), p20, TXAYZ ) 
Sabi MATMULC ( T6, TRPY, TXYZ ) 


C Compute the overall transformation, T: 


CALL MATMULA ( T, TO ) 
CALL MATMULA ( T, Tl ) 
Sach MATMULA ( T, T2 ) 
CALL MATMULA ( T, T3 ) 
CALL MATMULA ( T, T4 ) 
CALL MATMULA ( T, TS ) 
CALL MATMULA ( T, TO6 ) 
return 

end 


CK RK KKK KKK KKK KKK KKK KKK KKK KK KKK KKK KKK KKK KKK KKK KKK KKK KKK KKKKKKKKKK KK KK 
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